//----------------------------------------------------------------------------------------------
//	Filename:	StructureGraph.h
//	Author:		Sandro Spina
//	Date:		27/01/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------

#include "StructureGraph.h"
#include "StructureGraphsForge.h"
#include "PointCloudSegmentation.h"
#include "GraphCut.h"
#include <queue>
#include <boost\lexical_cast.hpp>
#include <boost\tokenizer.hpp>

#include <boost\interprocess\file_mapping.hpp>
#include <boost\interprocess\mapped_region.hpp>
#include <boost\filesystem\operations.hpp>
#include <boost\filesystem\path.hpp>
#include <boost\filesystem.hpp>

using namespace Illumina::Core;

StructureGraph::StructureGraph()
{		
	Initialise();
}

StructureGraph::StructureGraph(SegmentsGraph * _segmentsGraph)
{
	m_segmentsGraph = _segmentsGraph;
	Initialise();
}

void StructureGraph::Initialise()
{
	m_disjointStructureGraphs = new Illumina::Core::List<Node*>();
	//ConsumedStates = new std::stack<std::vector<int>*>();
	m_directionsTravelled = new std::map<std::string, int>();
	m_mergedNodesMap = new std::map<int, int>();
	m_mergedNodesSeeds = new std::map<int, std::vector<int>>();
	nodeID = new ::UniqueID();
	m_nodes = new std::map<int, Node*>();
}

//Create and populate a new node to represent a surface segment. Add to nodes map.
void StructureGraph::CreateNewNode(SegmentsGraph::SurfaceSegment * p_ss)
{
	Node * node = new Node();
	node->id = p_ss->id;
	node->segment = p_ss;
	node->stats = new NodeStats();
	node->stats->type = nodetype::level0;
	node->used = 0;	

	switch (p_ss->shape_primitive)
	{
		case SegmentsGraph::Shape::plane: node->stats->shape = StructureGraph::shape::plane; break;
		case SegmentsGraph::Shape::sphere: node->stats->shape = StructureGraph::shape::sphere; break;
		case SegmentsGraph::Shape::compound: node->stats->shape = StructureGraph::shape::compound; break;
		case SegmentsGraph::Shape::line: node->stats->shape = StructureGraph::shape::line; break;
		case SegmentsGraph::Shape::unknown: node->stats->shape = StructureGraph::shape::complex; break;
		default: node->stats->shape = StructureGraph::shape::notset;
	}
	
	node->stats->label = p_ss->label;		
	node->stats->point_count = p_ss->points->size();
	node->stats->bounding_min = p_ss->bounding_min;
	node->stats->bounding_max = p_ss->bounding_max;
	node->stats->mean_bounding_volume = (node->stats->bounding_min + node->stats->bounding_max) / 2;
	node->edges = new std::map<int, std::pair<EdgeStats*, Node*>*>();
	
	(*m_nodes)[p_ss->id] = node;
}	

void StructureGraph::PromoteInternalNodesToLevel0()
{
	//Iterate over all the nodes and create new level0 nodes for internal ones
	StructureGraph::Node * crt_node;
	StructureGraph::Node * crt_internal_node;
	StructureGraph * crt_internal_structure_graph;

	SegmentsGraph::SurfaceSegment * crt_segment;
	SegmentsGraph::SurfaceSegment * new_segment;
	SegmentsGraph::SurfaceSegment * crt_internal_segment;
	SegmentsGraph * crt_internal_segment_graph;

	std::vector<StructureGraph::Node *> nodesToRemove;
	std::map<int, int> surfaceSegmentsIdMapper;

	for (auto nodes_it = m_nodes->begin(); nodes_it != m_nodes->end(); nodes_it++)
	{		
		if (nodes_it->second->stats->shape != shape::compound) continue;   //this is not a compound node, i.e. no internal nodes so we can skip
		if (nodes_it->first == 201) 
		{
			std::cout << "" << std::endl;
		}
		crt_node = nodes_it->second;
		crt_segment = crt_node->segment;
		crt_internal_segment_graph = crt_segment->internalSegGraph;
		crt_internal_structure_graph = crt_segment->internalStrGraph;
		nodesToRemove.push_back(nodes_it->second);		//mark this for removal
		surfaceSegmentsIdMapper.clear();

		int biggest_segment = 0;
		int biggest_segment_point_count = 0;

		//Create new surface segment nodes in the segments graph first .. then structure graph node
		for (auto segments_it = crt_internal_segment_graph->m_surfaceSegments->begin(); segments_it != crt_internal_segment_graph->m_surfaceSegments->end(); segments_it++)
		{
			crt_internal_segment = segments_it->second;
			int new_segment_id = m_segmentsGraph->CreateNewSurfaceSegment(crt_internal_segment->shape_primitive);			
			surfaceSegmentsIdMapper[segments_it->first] = new_segment_id;
			new_segment = m_segmentsGraph->m_surfaceSegments->at(new_segment_id);
			new_segment->bounding_max = crt_internal_segment->bounding_max;
			new_segment->bounding_min = crt_internal_segment->bounding_min;
			new_segment->connections = crt_internal_segment->connections;
			new_segment->coverage = crt_internal_segment->coverage;
			new_segment->maxDecompositionCount = crt_internal_segment->maxDecompositionCount;
			new_segment->obb = crt_internal_segment->obb;
			new_segment->orientation = crt_internal_segment->orientation;
			new_segment->points = crt_internal_segment->points;
			new_segment->visibilityCount = crt_internal_segment->visibilityCount;
			//Finally change the allegiance of the points to the new segment
			for (int i=0; i<new_segment->points->size(); i++) m_segmentsGraph->m_pcs->m_pointCloud->m_pointList[new_segment->points->at(i)].m_segmentId = new_segment->id;
			if (new_segment->points->size() > biggest_segment_point_count) { biggest_segment = new_segment_id; biggest_segment_point_count = new_segment->points->size(); }
			//Do not establish connectivity with external segments for now.
			CreateNewNode(new_segment);
			(*(m_segmentsGraph->m_surfacePromotions))[new_segment_id] = crt_segment->id;
		}

		//check if this is a root node in the structure graph
		for (int i=0; i<m_disjointStructureGraphs->Size(); i++)
		{
			if (m_disjointStructureGraphs->At(i)->id == nodes_it->first)
			{
				m_disjointStructureGraphs->At(i) = m_nodes->at(biggest_segment);
			}
		}

		StructureGraph::Node * new_node;
		StructureGraph::Node * old_node;
		float intersection_score = 0.f;
		//copy connections between the newly created structure graph nodes and establish any with the previously present nodes.
		for (auto nodes_it = crt_internal_structure_graph->m_nodes->begin(); nodes_it != crt_internal_structure_graph->m_nodes->end(); nodes_it++)
		{
			new_node = m_nodes->at(surfaceSegmentsIdMapper[nodes_it->first]);
			old_node = nodes_it->second;
			for (auto edge_it = old_node->edges->begin(); edge_it != old_node->edges->end(); edge_it++)
			{
				std::cout << "Adding Connection btw " << new_node->id << " to " << m_segmentsGraph->m_surfaceSegments->at(surfaceSegmentsIdMapper[edge_it->first])->id << std::endl;
				AddNodeConnection(m_segmentsGraph->m_surfaceSegments->at(new_node->id), m_segmentsGraph->m_surfaceSegments->at(surfaceSegmentsIdMapper[edge_it->first]), edge_it->second->first->strength);
				AddNodeConnection(m_segmentsGraph->m_surfaceSegments->at(surfaceSegmentsIdMapper[edge_it->first]), m_segmentsGraph->m_surfaceSegments->at(new_node->id), edge_it->second->first->strength);
			}
			//Check if there are any OBB intersections between the new internal node and the previous level 0 ones.
			for (auto parent_node_edges_it = crt_node->edges->begin(); parent_node_edges_it != crt_node->edges->end(); parent_node_edges_it++)
			{
				intersection_score = m_segmentsGraph->InOBBPercentage(parent_node_edges_it->second->second->segment->obb, new_node->segment);
				if (intersection_score > 0.f)
				{					
					AddNodeConnection(parent_node_edges_it->second->second->segment, new_node->segment, new_node->segment->points->size() * intersection_score);
					AddNodeConnection(new_node->segment, parent_node_edges_it->second->second->segment, new_node->segment->points->size() * intersection_score);
				}
			}
		}
	}

	//Finally remove the previous compound nodes. Leave intact in the segments graph.
	StructureGraph::Node * nodeToRemove;
	StructureGraph::Node * connectedNode;
	for (auto toRemove_it = nodesToRemove.begin(); toRemove_it != nodesToRemove.end(); toRemove_it++)
	{
		//First remove incoming edges from connected nodes
		nodeToRemove = *toRemove_it;
		for (auto edge_it = nodeToRemove->edges->begin(); edge_it != nodeToRemove->edges->end(); edge_it++)
		{
			connectedNode = edge_it->second->second;
			auto conn_it = connectedNode->edges->find(nodeToRemove->id);
			if (conn_it != connectedNode->edges->end())
			{
				connectedNode->edges->erase(conn_it);
			}
		}
		//Finally remove this one.
		m_nodes->erase(nodeToRemove->id);
	}

}

std::vector<std::vector<int>> StructureGraph::GetNodesSortedBySize()
{
	std::vector<std::vector<int>> sorted;
	std::map<int, std::vector<int>> m;
	for (auto node : *m_nodes)
	{
		if (m_mergedNodesMap->find(node.second->id) == m_mergedNodesMap->end())
		{
			int s = node.second->segment->points->size();		
			m[s].push_back(node.second->id);
		}
	}

	for (auto node_ids : m) 
	{
		std::vector<int> grp;
		for (auto ids : node_ids.second) grp.push_back(ids);
		sorted.push_back(grp);
	}

	return sorted;
}


void StructureGraph::AddNodeConnection(SegmentsGraph::SurfaceSegment * p_segment_source, SegmentsGraph::SurfaceSegment * p_segment_sink, int p_strength) 
{
	Node * source = m_nodes->at(p_segment_source->id);
	Node * sink = m_nodes->at(p_segment_sink->id);
	EdgeStats * edge_stats = new EdgeStats();		
	edge_stats->label = "node";
	edge_stats->strength = p_strength;

	//Lines World Scene
	if (p_segment_source->shape_primitive == SegmentsGraph::line && p_segment_sink->shape_primitive == SegmentsGraph::line)
	{
		source->stats->shape = StructureGraph::shape::line;
		sink->stats->shape = StructureGraph::shape::line;
		edge_stats->dot = Maths::Abs(Illumina::Core::Vector3::Dot(Illumina::Core::Vector3::Normalize(p_segment_source->bounding_max - p_segment_source->bounding_min),
														Illumina::Core::Vector3::Normalize(p_segment_sink->bounding_max - p_segment_sink->bounding_min)));
		edge_stats->epsilon = 0.1f;
	}

	//3D Scene ... set angle between surfaces that are planar.
	if (p_segment_source->shape_primitive == SegmentsGraph::plane && p_segment_sink->shape_primitive == SegmentsGraph::plane)
	{
		source->stats->shape = StructureGraph::shape::plane;
		sink->stats->shape = StructureGraph::shape::plane;
		edge_stats->dot = Maths::Abs(Illumina::Core::Vector3::Dot(Illumina::Core::Vector3::Normalize(p_segment_source->orientation), Illumina::Core::Vector3::Normalize(p_segment_sink->orientation)));
		edge_stats->epsilon = 0.1f;
	}

	std::pair<EdgeStats*, Node*> * edge = new std::pair<EdgeStats*, Node*>(edge_stats, sink);
	if (source->edges->find(sink->id) != source->edges->end())
	{
		//Just increment strength
		source->edges->find(sink->id)->second->first->strength+=p_strength;
		//std::cout << "Overwriting an EDGE !! << src=" << source->id << " snk=" << sink->id << " Incrementing strength to " << source->edges->find(sink->id)->second->first->strength << std::endl;
	}
	else
	{
		//Add new sink node
		(*source->edges)[sink->id] = edge;
	}
}

//Returns a list of indices in breadth first order	
Illumina::Core::List<int> * StructureGraph::BreadthFirstTraversal(Node * p_initial)
{
	Illumina::Core::List<int> * traversal = new Illumina::Core::List<int>();
	std::map<int, bool> visited = std::map<int, bool>();
	for (m_nodesIterator = m_nodes->begin(); m_nodesIterator != m_nodes->end(); m_nodesIterator++)
	{
		visited[m_nodesIterator->first] = false;
	}

	std::queue<Node*> toVisit = std::queue<Node*>();
	toVisit.push(p_initial);
	while (!toVisit.empty())
	{
		traversal->PushBack(toVisit.front()->id);
		visited[toVisit.front()->id] = true;
		for (std::map<int, std::pair<EdgeStats*, Node*>*>::iterator toVisitIterator = toVisit.front()->edges->begin(); toVisitIterator != toVisit.front()->edges->end(); toVisitIterator++)
		//for (int i = 0; i < toVisit.front()->edges->size(); i++)
		{
			//if (!visited[toVisit.front()->edges->at(i)->second->id]) 
			if (!visited[toVisitIterator->first])
			{
				//toVisit.push(toVisit.front()->edges->at(i)->second);				
				//visited[toVisit.front()->edges->at(i)->second->id] = true;					
				toVisit.push(toVisitIterator->second->second);
				visited[toVisitIterator->first] = true;
			}
		}
		toVisit.pop();
	}
	return traversal;
}

std::map<int,std::vector<GraphCut*>*> * StructureGraph::SearchSceneGraph(std::string &query)
{
	StructureGraphsForge sgf;
	//Do some work(magic) here to transform the query into a structure graph.

	//Finished construction of structure graph
	return SearchSceneGraph(sgf.ForgeNullGraph());
}

//Merge Small states adjacent larger ones and make sure graph remains consistent with regards to connectivity.
void StructureGraph::MergeSmallStates(int p_min_number_of_points, SegmentsGraph * p_sg)
{	
	//Order surface nodes from biggest to smallest (still greater than p_min_number_of_points)
	std::vector<int> ordered_states; ordered_states.push_back(-1);
	std::vector<int> ordered_states_point_count; ordered_states_point_count.push_back(-1);
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count > p_min_number_of_points)
		{
			int insertion_idx=0;
			for (; insertion_idx<ordered_states.size(); insertion_idx++)
			{
				if (states_iterator->second->stats->point_count > ordered_states_point_count[insertion_idx])
					break;
			}
			ordered_states.insert(ordered_states.begin()+insertion_idx, states_iterator->first);
			ordered_states_point_count.insert(ordered_states_point_count.begin()+insertion_idx, states_iterator->second->stats->point_count);
		}
	}
	ordered_states.erase(ordered_states.end()-1); //Delete the initial -1 
	std::cout << "[MergeSmallStates] min_points= " << p_min_number_of_points << " #Big States = " << ordered_states.size() << " #Small States= " <<  m_nodes->size() - ordered_states.size() << std::endl;

	//Iterate over large states and 'eat'/merge smaller adjacent cells
	std::map<int, int> already_merged;
	std::vector<int> edges_to_delete;
	for (std::vector<int>::iterator ordered_iterator = ordered_states.begin(); ordered_iterator!=ordered_states.end(); ordered_iterator++)
	{
		Node* crt_state = m_nodes->at(*ordered_iterator);
		//Take in consideration transitive connections to small states.
		//Introduce while loop here ... Push transitive small states on a stack here. loop until stack is empty. $TODO$
		bool done = false;
		while (!done)
		{
			done = true;
			edges_to_delete.clear();
			for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connected_states = crt_state->edges->begin(); connected_states != crt_state->edges->end(); connected_states++)
			{
				std::pair<EdgeStats*, Node*> * crt_edge = connected_states->second;
				if (crt_edge->second->stats->point_count <= p_min_number_of_points)
				{
					//Merge sink state at crt_edge with crt_state
					if (already_merged.find(connected_states->first) == already_merged.end())
					{
						Node* crt_state_to_prune = m_nodes->at(connected_states->first);
						//Start merging ... First transfer points from small to big state
						std::vector<int> * transfer_points_from = p_sg->m_surfaceSegments->at(connected_states->first)->points;
						std::vector<int> * transfer_points_to = p_sg->m_surfaceSegments->at(crt_state->id)->points;
						for (std::vector<int>::iterator pi = transfer_points_from->begin(); pi != transfer_points_from->end(); pi++)
						{
							transfer_points_to->push_back(*pi);
						}
						crt_state->stats->point_count = transfer_points_to->size();						
						transfer_points_from->clear();

						already_merged[connected_states->first] = 1;  //Mark as merged
						edges_to_delete.push_back(connected_states->first); //mark for deletion from current state
						//Add all outgoing edges of the pruned state at crt_edge to the crt_state						
						for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator to_prune_connected_states = crt_state_to_prune->edges->begin(); 
											to_prune_connected_states != crt_state_to_prune->edges->end(); to_prune_connected_states++)
						{
							if (to_prune_connected_states->first != *ordered_iterator && //not cyclic and
								crt_state->edges->find(connected_states->first) == crt_state->edges->end() && //not already an edge
								already_merged.find(to_prune_connected_states->first) != already_merged.end()) //not already pruned
							{							
								//This edge leads to a currently active state
								crt_state->edges->insert(std::pair<int,std::pair<EdgeStats*, Node*> *>(to_prune_connected_states->first, to_prune_connected_states->second));
								//Have we merged a small state ... 
								if (crt_state_to_prune->stats->point_count <= p_min_number_of_points) done = false;
								//std::cout << "State " << crt_state->id << " eating up state " << crt_state_to_prune->id << std::endl;
							}
						}
					} 
					else
					{
						//Just remove the edge ... state was already merged to larger state previously
						edges_to_delete.push_back(connected_states->first);
					}
				}
			}
			//Finally remove edges to states which have been pruned
			for (std::vector<int>::iterator tdi = edges_to_delete.begin(); tdi != edges_to_delete.end(); tdi++)
			{
				crt_state->edges->erase(crt_state->edges->find(*tdi));
			}
		}
	}

	//Finally delete from nodes map all pruned states
	for (std::map<int, int>::iterator pruned_iterator = already_merged.begin(); pruned_iterator != already_merged.end(); pruned_iterator++)
	{
		m_nodes->erase(m_nodes->find(pruned_iterator->first));
	}

	std::cout << "[MergeSmallStates] #States Pruned= " << already_merged.size() << " - #Big States= " << m_nodes->size() << std::endl;
}

void StructureGraph::MergeBigStatesAcrossWeakEdges(int p_min_number_of_points, SegmentsGraph * p_sg)
{
	//Order surface nodes from biggest to smallest (still greater than p_min_number_of_points)
	std::vector<int> ordered_states; ordered_states.push_back(-1);
	std::vector<int> ordered_states_point_count; ordered_states_point_count.push_back(-1);
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		int insertion_idx=0;
		for (; insertion_idx<ordered_states.size(); insertion_idx++)
		{
			if (states_iterator->second->stats->point_count > ordered_states_point_count[insertion_idx])
				break;
		}
		ordered_states.insert(ordered_states.begin()+insertion_idx, states_iterator->first);
		ordered_states_point_count.insert(ordered_states_point_count.begin()+insertion_idx, states_iterator->second->stats->point_count);
	}
	
	ordered_states.erase(ordered_states.end()-1); //Delete the initial -1 
	std::cout << "[MergeBigStatesAcrossWeakEdges] min_points= " << p_min_number_of_points << " #Big States = " << ordered_states.size() << std::endl;

	//Iterate over states and merge with adjacent states if there's only one weak edge
	std::map<int, int> already_merged;
	std::vector<int> edges_to_delete;
	for (std::vector<int>::iterator ordered_iterator = ordered_states.begin(); ordered_iterator!=ordered_states.end(); ordered_iterator++)
	{
		Node* crt_state = m_nodes->at(*ordered_iterator);
		//Take in consideration transitive connections to small states.
		bool done = false;
		while (!done)
		{
			done = true;
			edges_to_delete.clear();
			for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connected_states = crt_state->edges->begin(); connected_states != crt_state->edges->end(); connected_states++)
			{
				std::pair<EdgeStats*, Node*> * crt_edge = connected_states->second;
				if (crt_edge->second->stats->point_count <= p_min_number_of_points)
				{
					//Merge sink state at crt_edge with crt_state
					if (already_merged.find(connected_states->first) == already_merged.end())
					{
						Node* crt_state_to_prune = m_nodes->at(connected_states->first);
						//Start merging ... First transfer points from small to big state
						std::vector<int> * transfer_points_from = p_sg->m_surfaceSegments->at(connected_states->first)->points;
						std::vector<int> * transfer_points_to = p_sg->m_surfaceSegments->at(crt_state->id)->points;
						for (std::vector<int>::iterator pi = transfer_points_from->begin(); pi != transfer_points_from->end(); pi++)
						{
							transfer_points_to->push_back(*pi);
						}
						crt_state->stats->point_count = transfer_points_to->size();						
						transfer_points_from->clear();

						already_merged[connected_states->first] = 1;  //Mark as merged
						edges_to_delete.push_back(connected_states->first); //mark for deletion from current state
						//Add all outgoing edges of the pruned state at crt_edge to the crt_state						
						for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator to_prune_connected_states = crt_state_to_prune->edges->begin(); 
											to_prune_connected_states != crt_state_to_prune->edges->end(); to_prune_connected_states++)
						{
							if (to_prune_connected_states->first != *ordered_iterator && //not cyclic and
								crt_state->edges->find(connected_states->first) == crt_state->edges->end() && //not already an edge
								already_merged.find(to_prune_connected_states->first) != already_merged.end()) //not already pruned
							{							
								//This edge leads to a currently active state
								crt_state->edges->insert(std::pair<int,std::pair<EdgeStats*, Node*> *>(to_prune_connected_states->first, to_prune_connected_states->second));
								//Have we merged a small state ... 
								if (crt_state_to_prune->stats->point_count <= p_min_number_of_points) done = false;
								//std::cout << "State " << crt_state->id << " eating up state " << crt_state_to_prune->id << std::endl;
							}
						}
					} 
					else
					{
						//Just remove the edge ... state was already merged to larger state previously
						edges_to_delete.push_back(connected_states->first);
					}
				}
			}
			//Finally remove edges to states which have been pruned
			for (std::vector<int>::iterator tdi = edges_to_delete.begin(); tdi != edges_to_delete.end(); tdi++)
			{
				crt_state->edges->erase(crt_state->edges->find(*tdi));
			}
		}
	}

	//Finally delete from nodes map all pruned states
	for (std::map<int, int>::iterator pruned_iterator = already_merged.begin(); pruned_iterator != already_merged.end(); pruned_iterator++)
	{
		m_nodes->erase(m_nodes->find(pruned_iterator->first));
	}

	std::cout << "[MergeSmallStates] #States Pruned= " << already_merged.size() << " - #Big States= " << m_nodes->size() << std::endl;
}

void StructureGraph::MergeBigStatesAcrossWeakEdges(SegmentsGraph * p_sg)
{
	//For each pair of states ... determine minimum number of points in edge

}

void StructureGraph::PruneSmallStates(int p_min_number_of_points)
{
	//Iterate over all states and remove those which have less points than p_min_number_of_points
	std::vector<int> to_prune;
	
	//Determine states to prune
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count < p_min_number_of_points)
			to_prune.push_back(states_iterator->first);
	}

	//Remove edges to pruned states
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
		{
			if (states_iterator->second->edges->find(*to_prune_iterator) != states_iterator->second->edges->end())
			{
				states_iterator->second->edges->erase(states_iterator->second->edges->find(*to_prune_iterator));
			}
		}
	}

	//Remove small states
	for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
	{
		m_nodes->erase(m_nodes->find(*to_prune_iterator));
	}
}

void StructureGraph::PruneBigStates(int p_max_number_of_points)
{
	//Iterate over all states and remove those which have more points than p_min_number_of_points
	std::vector<int> to_prune;
	
	//Determine states to prune
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		if (states_iterator->second->stats->point_count > p_max_number_of_points)
			to_prune.push_back(states_iterator->first);
	}

	//Remove edges to pruned states
	for (std::map<int, Node*>::iterator states_iterator = m_nodes->begin(); states_iterator != m_nodes->end(); states_iterator++)
	{
		for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
		{
			if (states_iterator->second->edges->find(*to_prune_iterator) != states_iterator->second->edges->end())
			{
				states_iterator->second->edges->erase(states_iterator->second->edges->find(*to_prune_iterator));
			}
		}
	}

	//Remove big states
	for (std::vector<int>::iterator to_prune_iterator = to_prune.begin(); to_prune_iterator != to_prune.end(); to_prune_iterator++)
	{
		m_nodes->erase(m_nodes->find(*to_prune_iterator));
	}
}

void StructureGraph::PruneWeakEdges(int p_min_number_of_points)
{
	//Iterate over all states and remove those which have less points than p_min_number_of_points
	std::vector<int> to_prune;
}

//Shape Structure Graphs Search
std::vector<std::vector<int>> StructureGraph::SearchCuboids()
{
	std::vector<std::vector<int>> cuboid_states;

	return cuboid_states;
}

std::vector<std::vector<int>> StructureGraph::SearchPlanes()
{
	std::vector<std::vector<int>> plane_states;
	
	return plane_states;
}

std::vector<std::vector<int>> StructureGraph::SearchSpheres()
{
	std::vector<std::vector<int>> sphere_states;

	return sphere_states;
}

std::vector<std::vector<int>> StructureGraph::SearchCylinders()
{
	std::vector<std::vector<int>> cylinder_states;

	return cylinder_states;
}

std::vector<std::vector<int>> StructureGraph::SearchObject(std::string p_shapeName)
{
	std::vector<std::vector<int>> object_states;

	return object_states;
}

std::vector<int> StructureGraph::AllDirectlyConnectedSegments(int _segmentFromID)
{
	std::vector<int> connectedSegments;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return connectedSegments;
	Node * crtNode = m_nodesIterator->second;
	for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connectedNodes = crtNode->edges->begin(); connectedNodes != crtNode->edges->end(); connectedNodes++)
	{
		//std::cout << "From#" << _segmentFromID << " To#" << connectedNodes->first << "[" << connectedNodes->second->second->segment->points.size() << "]" << " Dot->" << connectedNodes->second->first->dot << std::endl;
		connectedSegments.push_back(connectedNodes->first);		
	}
	connectedSegments.push_back(_segmentFromID);
	return connectedSegments;
}

std::vector<int> StructureGraph::AllTransitivelyConnectedSegments(int _segmentFromID)
{
	std::vector<int> connectedSegments;
	std::map<int,bool> connectedMap;
	bool checkMore = false;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return connectedSegments;
	Node * crtNode = m_nodesIterator->second;	
	connectedMap[_segmentFromID] = true;	

	for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connectedNodes = crtNode->edges->begin(); connectedNodes != crtNode->edges->end(); connectedNodes++)
	{		
		connectedSegments.push_back(connectedNodes->first);
		connectedMap[connectedNodes->first] = true;
		//std::cout << "From#" << _segmentFromID << " To#" << connectedNodes->first << "[" << connectedNodes->second->second->segment->points.size() << "]" << " Dot->" << connectedNodes->second->first->dot << std::endl;					
	}

	if (connectedSegments.size() > 0) checkMore = true;

	std::vector<int> tmpConnectedSegments;
	while (checkMore)
	{		
		checkMore = false;
		for (std::vector<int>::iterator conns = connectedSegments.begin(); conns != connectedSegments.end(); conns++)
		{
			for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connectedNodes = crtNode->edges->begin(); connectedNodes != crtNode->edges->end(); connectedNodes++)
			{
				if (connectedMap.find(connectedNodes->first) == connectedMap.end())
				{
					tmpConnectedSegments.push_back(connectedNodes->first);
					connectedMap[connectedNodes->first] = true;
					//std::cout << "From#" << _segmentFromID << " To#" << connectedNodes->first << "[" << connectedNodes->second->second->segment->points.size() << "]" << " Dot->" << connectedNodes->second->first->dot << std::endl;
				}
			}
		}

		if (tmpConnectedSegments.size() > 0) 
		{
			checkMore = true;
			connectedSegments.clear();
			for (int i=0; i<tmpConnectedSegments.size(); i++) connectedSegments.push_back(tmpConnectedSegments[i]);
			tmpConnectedSegments.clear();
		}
	}

	connectedSegments.clear();
	for (std::map<int, bool>::iterator seg_iter = connectedMap.begin(); seg_iter != connectedMap.end(); seg_iter++)
	{
		connectedSegments.push_back(seg_iter->first);
	}

	return connectedSegments;
}

std::vector<int> StructureGraph::DirectlyConnectedSegmentsAtAngle(int _segmentFromID, float _angle, float _epsilon)
{
	std::vector<int> connectedSegments;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return connectedSegments;
	Node * crtNode = m_nodesIterator->second;
	float min_angle = _angle - _epsilon;
	float max_angle = _angle + _epsilon;
	float d = 0;
	//std::cout << "Min Angle = " << min_angle << " - Max Angle = " << max_angle << std::endl;
	for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator connectedNodes = crtNode->edges->begin(); connectedNodes != crtNode->edges->end(); connectedNodes++)
	{
		//std::cout << "From#" << _segmentFromID << " To#" << connectedNodes->first << "[" << connectedNodes->second->second->segment->points.size() << "]" << " Dot->" << Maths::Abs(connectedNodes->second->first->dot) << std::endl;
		d = Vector3::Dot(crtNode->segment->obb.obb_0, connectedNodes->second->second->segment->obb.obb_0);				
		connectedNodes->second->first->dot = d;
		/*if (_segmentFromID == 0)
		{
			std::cout << " ++++++++++++++++ " << std::endl;
			std::cout << " +++ 0 -" << connectedNodes->second->second->id << " ... dot product =  " << connectedNodes->second->first->dot << " min_angle:" << min_angle << " .. max_angle:" << max_angle << std::endl;
			std::cout << " ++++++++++++++++ " << std::endl;
		}*/
		if ((Maths::Abs(d) >= min_angle) && (Maths::Abs(d) <= max_angle+0.001f))
		{				
			connectedSegments.push_back(connectedNodes->first);
		}

	}
	connectedSegments.push_back(_segmentFromID);
	return connectedSegments;
}

std::vector<int> StructureGraph::TransitivelyConnectedSegmentsAtAngle(int _segmentFromID, float _angle, float _epsilon)
{
	std::vector<int> connectedSegments;
	std::map<int,bool> connectedMap;
	bool checkMore = false;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return connectedSegments;
	Node * crtNode = m_nodesIterator->second;
	float min_angle = _angle - _epsilon;
	float max_angle = _angle + _epsilon;
	connectedMap[_segmentFromID] = true;
	//std::cout << "Min Angle = " << min_angle << " - Max Angle = " << max_angle << std::endl;
	std::queue<int> toProcess;
	toProcess.push(_segmentFromID);
	int nextSegment;

	while (!toProcess.empty())
	{
		nextSegment = toProcess.front();
		connectedSegments.clear();
		connectedSegments = DirectlyConnectedSegmentsAtAngle(nextSegment, _angle, _epsilon);
		for (int i=0; i<connectedSegments.size(); i++) 
		{
			if (connectedMap.find(connectedSegments[i]) == connectedMap.end())
			{
				connectedMap[connectedSegments[i]] = true;
				toProcess.push(connectedSegments[i]);
			}
		}
		toProcess.pop();
	}

	connectedSegments.clear();
	for (std::map<int, bool>::iterator seg_iter = connectedMap.begin(); seg_iter != connectedMap.end(); seg_iter++) connectedSegments.push_back(seg_iter->first);

	return connectedSegments;
}

//Returns directly connected and on same plane surface segments
std::vector<int> StructureGraph::DirectlyConnectedCoplanarSegments(int _segmentFromID, float _angle, float _epsilon)
{	
	std::vector<int> onSamePlane;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return onSamePlane;	
	SegmentsGraph::OBB srcOBB = m_nodesIterator->second->segment->obb;	 
	Plane src_median_plane(srcOBB.p_0_min); 
	src_median_plane.Distance = (srcOBB.p_0_min.Distance + srcOBB.p_0_max.Distance) / 2;
	float coplanarDst = m_segmentsGraph->m_pcs->m_pointCloud->GetCoplanarDst();
	
 	std::vector<int> connectedSegments = DirectlyConnectedSegmentsAtAngle(_segmentFromID, _angle, _epsilon);
	SegmentsGraph::OBB crtOBB;
	Plane crt_median_plane;
	//std::cout << "[Coplanar segments] ------------ connected count = " << connectedSegments.size() << ", coplanarDst=" << coplanarDst << std::endl;
	//std::cout << "[Coplanar segments] Src" << srcOBB.obb_0.ToString() << "\n***p0min=" << srcOBB.p_0_min.ToString() << "\n***p0max=" << srcOBB.p_0_max.ToString() << " " << std::endl;
	//std::cout << "[Coplanar segments] Src" << srcOBB.obb_1.ToString() << "\n***p1min=" << srcOBB.p_1_min.ToString() << "\n***p1max=" << srcOBB.p_1_max.ToString() << " " << std::endl;
	//std::cout << "[Coplanar segments] Src" << srcOBB.obb_2.ToString() << "\n***p2min=" << srcOBB.p_2_min.ToString() << "\n***p2max=" << srcOBB.p_2_max.ToString() << " " << std::endl;
	Node *crt;

	//std::cout << "[DirectlyConnectedCoPlanarSegments] Retrieved " << connectedSegments.size() << " directly connected to " << _segmentFromID << std::endl;
	for (int i=0; i<connectedSegments.size(); i++)
	{
		crt = m_nodes->find(connectedSegments[i])->second;
		crtOBB = crt->segment->obb;
		crt_median_plane = crtOBB.p_0_min;
		
		crt_median_plane.Distance = ((crtOBB.p_0_min.Distance + crtOBB.p_0_max.Distance) / 2) * Maths::ISgn(Vector3::Dot(srcOBB.obb_0, crtOBB.obb_0));
		//std::cout << "[Coplanar segments] #" << crt->id << " MinDist=" << crtOBB.p_0_min.Distance << " MaxDist=" << crtOBB.p_0_max.Distance << " MidDist=" << crt_median_plane.Distance << " Diff=" << Maths::Abs(src_median_plane.Distance - crt_median_plane.Distance) << std::endl;		
		//std::cout << "[Coplanar segments] Crt" << crtOBB.obb_1.ToString() << "\n***p1min=" << crtOBB.p_1_min.ToString() << "\n***p1max=" << crtOBB.p_1_max.ToString() << " " << std::endl;
		//std::cout << "[Coplanar segments] Crt" << crtOBB.obb_2.ToString() << "\n***p2min=" << crtOBB.p_2_min.ToString() << "\n***p2max=" << crtOBB.p_2_max.ToString() << " " << std::endl;
		//std::cout << "++++" << std::endl;

		if (Maths::Abs(src_median_plane.Distance - crt_median_plane.Distance) <= coplanarDst) onSamePlane.push_back(connectedSegments[i]);
	}

	//std::cout << "[Coplanar segments] ------------ coplanar count = " << onSamePlane.size() << std::endl;
	return onSamePlane;
}

//Returns all transitively connected and on same plane surface segments
std::vector<int> StructureGraph::TransitivelyConnectedCoplanarSegments(int _segmentFromID, float _angle, float _epsilon)
{
	std::vector<int> connectedSegments;
	std::map<int,bool> connectedMap;
	bool checkMore = false;
	m_nodesIterator = m_nodes->find(_segmentFromID);
	if (m_nodesIterator == m_nodes->end()) return connectedSegments;
	Node * crtNode = m_nodesIterator->second;
	float min_angle = _angle - _epsilon;
	float max_angle = _angle + _epsilon;
	connectedMap[_segmentFromID] = true;
	std::queue<int> toProcess;
	toProcess.push(_segmentFromID);
	int nextSegment;

	while (!toProcess.empty())
	{
		nextSegment = toProcess.front();
		connectedSegments.clear();
		connectedSegments = DirectlyConnectedCoplanarSegments(nextSegment, _angle, _epsilon);
		for (int i=0; i<connectedSegments.size(); i++) 
		{
			if (connectedMap.find(connectedSegments[i]) == connectedMap.end())
			{
				connectedMap[connectedSegments[i]] = true;
				toProcess.push(connectedSegments[i]);
			}
		}
		toProcess.pop();
	}

	connectedSegments.clear();
	for (std::map<int, bool>::iterator seg_iter = connectedMap.begin(); seg_iter != connectedMap.end(); seg_iter++) connectedSegments.push_back(seg_iter->first);

	return connectedSegments;	
}

//Merge all nodes into the first state of _nodes vector
void StructureGraph::MergeStates(std::vector<int> _nodes, int _level)
{	
	if (_nodes.size() < 2) return;
	std::map<int, Node*>::iterator nodeIterator = m_nodes->find(_nodes.front());
	if (nodeIterator == m_nodes->end()) return;
	Node * firstNode = nodeIterator->second;
	SegmentsGraph::SurfaceSegment * firstSegment = firstNode->segment;
	std::cout << "[MERGE STATES] Head Node#" << firstNode->id << " Total States=" << _nodes.size() << std::endl;
	if (m_mergedNodesSeeds->find(firstNode->id) == m_mergedNodesSeeds->end()) (*m_mergedNodesSeeds)[firstNode->id] = std::vector<int>();

	SegmentsGraph::SurfaceSegment * toMergeSegment;	
	//Merging is carried out on the structure graph by mapping state ids...
	for (int i = 1; i<_nodes.size(); i++)
	{
		nodeIterator = m_nodes->find(_nodes[i]);
		if (nodeIterator==m_nodes->end()) continue;	//node does not exist 
		if (m_mergedNodesMap->find(_nodes[i]) != m_mergedNodesMap->end()) continue;  //node is already merged ... cannot be remerged!!
		toMergeSegment = nodeIterator->second->segment;
		//Update Point SegmentId
		for (int j=0; j<toMergeSegment->points->size(); j++)
		{
			if (_level == 0)
				m_segmentsGraph->m_pcs->m_pointCloud->m_pointList[toMergeSegment->points->at(j)].m_segmentId = firstSegment->id;
			else
				m_segmentsGraph->m_pcs->m_pointCloud->m_pointList[toMergeSegment->points->at(j)].m_segmentId2 = firstSegment->id;
		}
		//std::cout << "Merging Node#" << toMergeSegment->id << " with Node#" << firstSegment->id << std::endl;
		(*m_mergedNodesMap)[toMergeSegment->id] = firstSegment->id;
		(*m_mergedNodesSeeds)[firstNode->id].push_back(toMergeSegment->id);
	}

	//Update OBB of the merged into node
	firstSegment->obb = m_segmentsGraph->ComputeOBB(_nodes, 0.5f);
}

//Process the entire graph and merge coplanar surface segments .. returns the ratio no. of states before / no. of states after merging
float StructureGraph::AutoMergeCoplanar(float _angle, float _epsilon, int _level)
{
	std::map<int, bool> visited;
	Node * crtNode;
	std::vector<int> conn_nodes;
	std::vector<int> merge_states;
	int nodes_count = 0;
	int merge_count = 0;
	
	for (std::map<int, Node*>::iterator nodes_iterator = m_nodes->begin(); nodes_iterator != m_nodes->end(); nodes_iterator++)
	{
		nodes_count++;
		if (visited.find(nodes_iterator->first) != visited.end()) continue;
		
		visited[nodes_iterator->first] = true;
		conn_nodes.clear();
		conn_nodes = TransitivelyConnectedCoplanarSegments(nodes_iterator->first, _angle, _epsilon);
		for (int i=0; i<conn_nodes.size(); i++) visited[conn_nodes[i]] = true;  //Mark as visited
		
		//Identify the biggest state.
		int max_index = 0; int max = 0; int crt = 0;
		for (int i=0; i<conn_nodes.size(); i++)
		{			
			crt = m_segmentsGraph->m_surfaceSegments->at(conn_nodes[i])->points->size();
			if (crt > max) { max = crt; max_index = i; }
		}

		merge_count += (conn_nodes.size() - 1);
		//Merge coplanar connected surface states
		merge_states.clear(); merge_states.push_back(conn_nodes.at(max_index));
		for (int i=0; i<conn_nodes.size(); i++) if (i!=max_index) merge_states.push_back(conn_nodes[i]);
		SegmentsGraph::OBB newObb = m_segmentsGraph->ComputeOBB(merge_states, 0.1f);
		if (Maths::Abs(newObb.p_0_max.Distance - newObb.p_0_min.Distance) < 2.0f) 
			MergeStates(merge_states, _level);	//Only merge if the obb extent along the U axis is reasonable otherwise it fucks up grid matching.
	}

	std::cout << "[AutoMerge Coplanar] Original Node Count = " << nodes_count << ", Merge Count=" << merge_count << std::endl;
	return nodes_count / (nodes_count-merge_count);
}

bool StructureGraph::IsNodeMerged(int _nodeId)
{
	if (m_mergedNodesMap->find(_nodeId) != m_mergedNodesMap->end()) return true;
	return false;
}

std::vector<SegmentsGraph::SurfaceSegment*> StructureGraph::NodeMergedWith(int _nodeId)
{
	std::vector<SegmentsGraph::SurfaceSegment*> mergedNodes;
	for (auto merge_it = m_mergedNodesMap->begin(); merge_it != m_mergedNodesMap->end(); merge_it++)
	{
		if (merge_it->second == _nodeId) mergedNodes.push_back(m_segmentsGraph->m_surfaceSegments->at(merge_it->first));
	}
	return mergedNodes;
}

std::pair<float, float> StructureGraph::Minimise(float _angle, float _epsilon)
{
	std::pair<float, float> ratios;
	std::vector<int> ordered_by_size;
	std::map<int, bool> node_assigned;
	for (std::map<int, Node*>::iterator nodes_iterator = m_nodes->begin(); nodes_iterator != m_nodes->end(); nodes_iterator++)
	{

	}
	return ratios;
}

//Graph Search Functions. map<node id, node>
//std::map<int,std::vector<std::vector<int>*>*> * SearchSceneGraph(std::pair<int, std::map<int, Node *>*> * structure)
std::map<int,std::vector<GraphCut*>*> * StructureGraph::SearchSceneGraph(ObjStructureGraph * structure)
{
	int initial = structure->GetInitialNodeId();
	std::map<int, ObjStructureGraph::Node*> * p_structureNodes = structure->GetNodes(); 

	//structures stores the graph cuts from the scene structure graph which match with the object input structure
	std::map<int, std::vector<std::vector<int>*>*> * structures = new std::map<int, std::vector<std::vector<int>*>*>();
	std::map<int, std::vector<GraphCut *> *> * structures2 = new std::map<int, std::vector<GraphCut *> *>();
		
	//stackTrace stores the suffix of the graph cut up to the point when the node is added to the stack
	std::stack<std::pair<int, int>> stack; 
	std::stack<std::pair<int, int>> stack2; //store the previous states to those in stack	
	
	std::stack<std::vector<int>> stackTrace; 
	std::stack<std::vector<int>> stackTraceObj; //the stack should keep trace of both the state and the outgoing transition  
	std::stack<std::vector<std::pair<std::pair<int,int>, int>>> stackTraceFull;

	std::stack<std::vector<int>> altClosedPaths; 
	std::stack<int> edgeStack;
		
	//store indices of current list of nodes making up this cut
	std::vector<int> crtGraphCut;
	std::vector<int> crtGraphCutObj;
	std::vector<int> crtGraphCutObjEdges;
	std::vector<ObjStructureGraph::Node *> crtGraphCutObjNodes;

	//std::vector<ObjStructureGraph::Node *> * crtGraphCutObjNodes = new std::vector<ObjStructureGraph::Node *>();

	ObjStructureGraph::EdgeStats * es;

	int prevSceneNodeId = 0;
	int prevStructureNodeId = 0;

	int crtStructureNodeId = initial;
	int crtSceneNodeId = 0;
	int crtStructureNodeEdgeId = -1;

	std::vector<std::pair<int, int>> * compatible = new std::vector<std::pair<int, int>>();
	std::vector<int> * edgeIndices = new std::vector<int>();

	std::map<std::string, bool> visitedpairs = std::map<std::string, bool>();
	std::map<int, bool> visitedstates = std::map<int, bool>();
	std::vector<int> tempStore = std::vector<int>();

	std::vector<ObjStructureGraph::action> * crtActions = new std::vector<ObjStructureGraph::action>();

	std::pair<int, int> crtPair;
	std::pair<int, int> prevPair;

	int structuresMapKey = 0;

	//search for stucture - naively iterate over all the nodes in the scene disjoint structure graphs for now.
	for (int dgraph = 0; dgraph < m_disjointStructureGraphs->Size(); dgraph++)
	{
			//get an enumaration of the nodes in this scene disjoint structure graph
			Illumina::Core::List<int> * bft_nodes = BreadthFirstTraversal(m_disjointStructureGraphs->At(dgraph));
			for (int nodeCount = 0; nodeCount < bft_nodes->Size(); nodeCount++)
			{
				//clear structures and add the next node to the stack ... which should initially (here) be empty
				if (m_nodes->at(bft_nodes->At(nodeCount))->used == 1) 
				{						
					std::cout << "Skipping Consumed Node :: " << bft_nodes->At(nodeCount) << std::endl;
					continue;
				}
				visitedstates.clear();
				visitedpairs.clear();
				crtGraphCut.clear();
				crtGraphCutObj.clear();
				crtGraphCutObjEdges.clear();
				//crtGraphCutObjNodes->clear();
				GraphCut * crtGCut = new GraphCut(boost::lexical_cast<std::string>(initial));				
				stack.push(std::pair<int,int>(initial,bft_nodes->At(nodeCount)));
				stack2.push(std::pair<int,int>(initial,bft_nodes->At(nodeCount)));
				
				stackTrace.push(std::vector<int>());
				stackTraceObj.push(std::vector<int>());
				stackTraceFull.push(std::vector<std::pair<std::pair<int, int>, int>>());
				
				altClosedPaths.push(std::vector<int>());
				//crtGraphCut.push_back(bft_nodes->At(nodeCount));
				std::cout << "**************************************" << std::endl;
				std::cout << "Starting new search [" << initial << "," << bft_nodes->At(nodeCount) << "]" << std::endl;
				std::vector<std::vector<int>*>* GraphCutsAtNode = new std::vector<std::vector<int>*>();
				std::vector<GraphCut *> * GCutsAtNode = new std::vector<GraphCut *>();
				structuresMapKey = bft_nodes->At(nodeCount);

				while (!stack.empty())
				{
					//Get the next element from the stack
					crtPair = stack.top();	stack.pop();
					prevPair = stack2.top();  stack2.pop();
					prevSceneNodeId = prevPair.second;
					prevStructureNodeId = prevPair.first;					
					crtSceneNodeId = crtPair.second;
					crtStructureNodeId = crtPair.first;					
					std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);
					//std::cout << "Stack Pop [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
					std::vector<int> tracePrefix = stackTrace.top(); stackTrace.pop();
					std::vector<int> tracePrefixObj = stackTraceObj.top(); stackTraceObj.pop();
					std::vector<std::pair<std::pair<int,int>, int>> tracePrefixFull = stackTraceFull.top(); stackTraceFull.pop();
					visitedstates.clear();	
					for (int j = 0; j < tracePrefixFull.size(); j++) 
					{	
						//crtGraphCut.push_back(tracePrefix[j]); //currentGraphCut Updated.
						//crtGraphCutObj.push_back(tracePrefixObj[j]);
						crtGraphCutObj.push_back(tracePrefixFull[j].first.first);
						crtGraphCut.push_back(tracePrefixFull[j].first.second);
						crtGraphCutObjEdges.push_back(tracePrefixFull[j].second);
						crtGCut->AddNode(m_nodes->at(tracePrefix[j]));
						visitedstates[tracePrefix[j]] = true;
						crtGraphCutObjNodes.push_back(p_structureNodes->at(tracePrefixObj[j]));
					}
					//Execute actions for transitions so far (if size is greater than 0) ...
					if (crtGraphCut.size() > 0)
						crtGCut->ExecuteActionsFromTrace(crtGraphCutObjNodes, &crtGraphCut, &crtGraphCutObjEdges);

					//Ban options from the previous two paths.
					std::vector<int> altOptions = altClosedPaths.top(); altClosedPaths.pop();
					for (int k = 0; k < altOptions.size(); k++)
					{
						visitedstates[altOptions[k]] = true;
					}					

					visitedpairs[vkey] = true;
					visitedstates[crtSceneNodeId] = true;
					crtGraphCut.push_back(crtSceneNodeId);
					crtGraphCutObj.push_back(crtStructureNodeId);
					crtGCut->AddNode(m_nodes->at(crtSceneNodeId));
					if (!edgeStack.empty())
					{
						//Execute Actions associated with this move ...
						es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeStack.top())->first; 
						crtGraphCutObjEdges.push_back(edgeStack.top());
						edgeStack.pop();
						crtGCut->ExecuteActions(es->actions, m_nodes->at(prevSceneNodeId), m_nodes->at(crtSceneNodeId));
					}
					else 
					{
						//this is the first pair of states ... push -1 to the obj edges stack
						crtGraphCutObjEdges.push_back(-1);
					}
					//..now check compatibilities and update stack in case of non-deterministic choice.
					while (EdgesCompatible(p_structureNodes->at(crtStructureNodeId), m_nodes->at(crtSceneNodeId), compatible, edgeIndices))
					{	
						//Remove previosly visited pairs
						tempStore.clear(); int idx = 0;
						for (std::vector<std::pair<int, int>>::iterator compatibleIterator = compatible->begin(); compatibleIterator != compatible->end(); compatibleIterator++, idx++)
						{
							 std::string vk = boost::lexical_cast<std::string, int> (compatibleIterator->first) + ":" + boost::lexical_cast<std::string, int> (compatibleIterator->second);
							 //if (visitedpairs.find(vk) != visitedpairs.end()) tempStore.push_back(idx);
							 if (visitedstates.find(compatibleIterator->second) != visitedstates.end()) tempStore.push_back(idx);
						}
						for (idx = tempStore.size(); idx > 0; idx--)
						{
							compatible->erase(compatible->begin()+tempStore[idx-1]);
							edgeIndices->erase(edgeIndices->begin()+tempStore[idx-1]);
						}
						//Done.


						if (compatible->size() == 0)
						{ 
							//std::cout << "Closing Path [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl; 
							break; 
						}

						if (compatible->size() == 1) 
						{
							//update cut sequence, crtStructureNodeId and crtSceneNodeId.
							prevSceneNodeId = crtSceneNodeId;
							prevStructureNodeId = crtStructureNodeId;							
							crtStructureNodeId = compatible->front().first;
							crtSceneNodeId = compatible->front().second;
							std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);							
							crtGraphCut.push_back(crtSceneNodeId);
							crtGraphCutObj.push_back(crtStructureNodeId);
							crtGraphCutObjEdges.push_back(edgeIndices->front());
							crtGCut->AddNode(m_nodes->at(crtSceneNodeId));
							visitedpairs[vkey] = true;
							visitedstates[crtSceneNodeId] = true;
							/*Carry out the actions linked with this edge*/
							es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeIndices->front())->first;							
							crtGCut->ExecuteActions(es->actions, m_nodes->at(prevSceneNodeId), m_nodes->at(crtSceneNodeId));
							//std::cout << "One Compatible Edge [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
							continue;
						}

						if (compatible->size() > 1) 
						{
							prevSceneNodeId = crtSceneNodeId;
							prevStructureNodeId = crtStructureNodeId;
							//Save state for alt-deterministic choice
							for (int x = 1; x < compatible->size(); x++)
							{
								crtStructureNodeId = compatible->at(x).first;
								crtSceneNodeId = compatible->at(x).second;
								std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);															
								visitedpairs[vkey] = true;
								visitedstates[crtSceneNodeId] = true;
								//std::cout << "Pushing to Stack [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
								stack.push(std::pair<int,int>(compatible->at(x).first, compatible->at(x).second));
								stack2.push(std::pair<int,int>(prevStructureNodeId, prevSceneNodeId));								
								edgeStack.push(edgeIndices->at(x));

								std::vector<int> prefixCut;
								std::vector<int> prefixCutObj;
								std::vector<std::pair<std::pair<int,int>, int>> prefixCutFull;
								std::vector<int> altPaths;
								
								for (int x2 = 0; x2 < compatible->size(); x2++)
								{
									if (x2 != x) altPaths.push_back(compatible->at(x2).second);
								}
								altClosedPaths.push(altPaths);
								for (int j = 0; j < crtGraphCut.size(); j++) prefixCut.push_back(crtGraphCut[j]); //currentGraphCut Updated.
								for (int j = 0; j < crtGraphCutObj.size(); j++) prefixCutObj.push_back(crtGraphCutObj[j]); //currentGraphCut Updated.
								for (int j = 0; j < crtGraphCut.size(); j++) prefixCutFull.push_back(std::pair<std::pair<int,int>, int>(std::pair<int,int>(crtGraphCutObj[j], crtGraphCut[j]), crtGraphCutObjEdges[j])); //currentGraphCut Updated.

								stackTrace.push(prefixCut);
								stackTraceObj.push(prefixCutObj);
								stackTraceFull.push(prefixCutFull);
							}

							//Choose first compatible pair, update cut sequence, crtStructureNodeId and crtSceneNodeId.
							crtStructureNodeId = compatible->front().first;
							crtSceneNodeId = compatible->front().second;
							std::string vkey = boost::lexical_cast<std::string, int> (crtStructureNodeId) + ":" + boost::lexical_cast<std::string, int> (crtSceneNodeId);
							crtGraphCut.push_back(crtSceneNodeId);
							crtGraphCutObj.push_back(crtStructureNodeId);
							crtGraphCutObjEdges.push_back(edgeIndices->front());
							crtGCut->AddNode(m_nodes->at(crtSceneNodeId));
							visitedstates[crtSceneNodeId] = true;
							visitedpairs[vkey] = true;
							/*Carry out the actions linked with this edge*/
							es = p_structureNodes->at(prevStructureNodeId)->edges->at(edgeIndices->front())->first;
							crtGCut->ExecuteActions(es->actions, m_nodes->at(prevSceneNodeId), m_nodes->at(crtSceneNodeId));
							//std::cout << "Choosing Path [" << crtStructureNodeId << "," << crtSceneNodeId << "]" << std::endl;
							continue;
						}
					}

					//No more compatible edges found in this cut - there might still be some valid cut prefixes in the stack.
					//If this cut is at least greater than 1 node, include in the structures list.
					//Eventually it will be added only if certain conditions are met. e.g. exercising of final states
					if (crtGraphCut.size() > 1 && p_structureNodes->at(crtStructureNodeId)->stats->type == StructureGraph::nodetype::level0) 
					{
						std::vector<int> * cut = new std::vector<int>();						
						for (std::vector<int>::iterator it = crtGraphCut.begin(); it != crtGraphCut.end(); it++)
						{
							cut->push_back(*it);
						}
						GraphCutsAtNode->push_back(cut);
						crtGCut->orderedCutNodes = cut;
						GCutsAtNode->push_back(crtGCut);
						crtGraphCut.clear();
						crtGraphCutObj.clear();
						crtGraphCutObjEdges.clear();
						crtGraphCutObjNodes.clear();
						crtGCut = new GraphCut(boost::lexical_cast<std::string>(initial));
					} 
					else 
					{
						crtGCut->Clear();
						crtGraphCut.clear();
						crtGraphCutObj.clear();
						crtGraphCutObjEdges.clear();
						crtGraphCutObjNodes.clear();
					}
				}
				//Finish working on this node
				std::vector<std::vector<int>*> * cuts = new std::vector<std::vector<int>*>();
				for (std::vector<std::vector<int>*>::iterator it = GraphCutsAtNode->begin(); it != GraphCutsAtNode->end(); it++)
				{
					cuts->push_back(*it);
				}
				(*structures)[structuresMapKey] = cuts;

				(*structures2)[structuresMapKey] = GCutsAtNode;
				
				GraphCutsAtNode->clear();				
			}			

		}

		return structures2;
	}		
	
//Given two nodes, returns pairs of compatible edges.
bool StructureGraph::EdgesCompatible(ObjStructureGraph::Node * p_nodeStructure, StructureGraph::Node * p_nodeScene, std::vector<std::pair<int, int>> * compatible, std::vector<int> * edgeIndices) 
{
	compatible->clear();
	edgeIndices->clear();
	//std::cout << "Edges Compatible between " << p_nodeStructure->id << " and " << p_nodeScene->id << std::endl;
	for (int i = 0; i < p_nodeStructure->edges->size(); i++)
	{
		for (int j = 0; j < p_nodeScene->edges->size(); j++)
		{
			if (EdgeCompatible(p_nodeStructure, i, p_nodeScene, j))
			{
				compatible->push_back(std::pair<int,int>(p_nodeStructure->edges->at(i)->second->id,
															p_nodeScene->edges->at(j)->second->id));
				edgeIndices->push_back(i);
				//std::cout << "[" << p_nodeStructure->edges->at(i)->second->id << "," << p_nodeScene->edges->at(j)->second->id << "]" << " Dot Product = " << p_nodeStructure->edges->at(i)->first->dot << std::endl;
			}
		}
	}
	if (compatible->size() > 0) return true;
	return false;
}	

//Check compatibility between p_edge1 of p_node1 and p_edge2 of p_node2
bool StructureGraph::EdgeCompatible(ObjStructureGraph::Node * p_nodeStructure, int p_edge1, StructureGraph::Node * p_nodeScene, int p_edge2) 
{
	bool dotcompatible = false;
	if ((p_nodeStructure->edges->at(p_edge1)->first->dot+p_nodeStructure->edges->at(p_edge1)->first->epsilon >
		p_nodeScene->edges->at(p_edge2)->first->dot) && (p_nodeScene->edges->at(p_edge2)->first->dot > 
		p_nodeStructure->edges->at(p_edge1)->first->dot-p_nodeStructure->edges->at(p_edge1)->first->epsilon))
		dotcompatible = true;

	return dotcompatible;
}

//**********Serialisation Functions********************
void StructureGraph::WriteToBSTRFile(std::string _filepath)
{
	std::string pcsPath = _filepath+".bstr";		
	const char * filename = pcsPath.c_str();	
	
	int numberOfNodes = m_nodes->size();	

	//Determine size of file first
	int sizeNodes = sizeof(int);
	for (m_nodesIterator = m_nodes->begin(); m_nodesIterator != m_nodes->end(); m_nodesIterator++) 
		sizeNodes+=m_nodesIterator->second->sizeInBytes();
	
	int sizeDisjointGraphs = (m_disjointStructureGraphs->Size() * sizeof(int)) + sizeof(int);

	int sizeMergedNodesMap = (m_mergedNodesMap->size() * (sizeof(int) + sizeof(int))) + sizeof(int);
	int sizeMergedNodes_ = (m_mergedNodesSeeds->size() * (sizeof(int) + sizeof(int))) + sizeof(int);

	size_t file_size = sizeNodes + sizeDisjointGraphs + sizeMergedNodesMap + sizeMergedNodes_;

	std::filebuf fbuf;
	fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);	
	fbuf.pubseekoff( file_size - sizeof(char), std::ios_base::beg);
	fbuf.sputc(0);
	fbuf.close();

	boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
	boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

	int * r_addr_i = (int*)region.get_address();			
	float * r_addr_f;
	size_t r_size = region.get_size();				
	std::cout << "[StructureGraph        ] Writing Bin File " << pcsPath << ". AllocSize= [" << r_size << "]" << std::endl;		

	int intsize = sizeof(int);
	int floatsize = sizeof(float);
	Int64 file_start = (Int64)r_addr_i;

	//std::cout << " size of int = " << intsize << std::endl;
	//std::cout << " size of float = " << floatsize << std::endl;
	//std::cout << " size of nodetype = " << sizeof(nodetype) << std::endl;
	//std::cout << " size of shape = " << sizeof(shape) << std::endl;

	//Start writing nodes
	*r_addr_i = numberOfNodes; r_addr_i++;
	//std::cout << "NumberOfNodes=" << numberOfNodes << std::endl;	
	for (m_nodesIterator = m_nodes->begin(); m_nodesIterator != m_nodes->end(); m_nodesIterator++) 
	{		
		int consumed = 0; 
		//std::cout << "Node Size Reserved = " << m_nodesIterator->second->sizeInBytes() << " ... " << std::endl;
		*r_addr_i = m_nodesIterator->second->id; r_addr_i++; consumed += intsize;
		//std::cout << "Id=" << m_nodesIterator->second->id << std::endl;
		*r_addr_i = m_nodesIterator->second->used; r_addr_i++; consumed += intsize;
		//std::cout << "Used=" << m_nodesIterator->second->used << std::endl;
		*r_addr_i = m_nodesIterator->second->segment->id; r_addr_i++; consumed += intsize;
		//std::cout << "SegId=" << m_nodesIterator->second->segment->id << std::endl;
		*r_addr_i = m_nodesIterator->second->stats->point_count; r_addr_i++; consumed += intsize;
		//std::cout << "PointCount=" << m_nodesIterator->second->stats->point_count << std::endl;
		*r_addr_i = m_nodesIterator->second->stats->shape; r_addr_i++; consumed += intsize;
		//std::cout << "Shape=" << m_nodesIterator->second->stats->shape << std::endl;
		*r_addr_i = m_nodesIterator->second->stats->type; r_addr_i++; consumed += intsize;
		//std::cout << "Type=" << m_nodesIterator->second->stats->type << std::endl;
		r_addr_f = (float*) r_addr_i;
		
		*r_addr_f = m_nodesIterator->second->stats->orientation.X; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->orientation.Y; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->orientation.Z; r_addr_f++; consumed += floatsize;

		*r_addr_f = m_nodesIterator->second->stats->bounding_min.X; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->bounding_min.Y; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->bounding_min.Z; r_addr_f++; consumed += floatsize;
		
		*r_addr_f = m_nodesIterator->second->stats->bounding_max.X; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->bounding_max.Y; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->bounding_max.Z; r_addr_f++; consumed += floatsize;
				
		*r_addr_f = m_nodesIterator->second->stats->mean_bounding_volume.X; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->mean_bounding_volume.Y; r_addr_f++; consumed += floatsize;
		*r_addr_f = m_nodesIterator->second->stats->mean_bounding_volume.Z; r_addr_f++; consumed += floatsize;
		
		r_addr_i = (int*) r_addr_f;
		*r_addr_i = m_nodesIterator->second->edges->size(); r_addr_i++; consumed += intsize;

		//std::cout << "Structure Graph Edges Count - " << m_nodesIterator->second->edges->size() << std::endl;

		for (std::map<int, std::pair<EdgeStats*, Node*> *>::iterator it = m_nodesIterator->second->edges->begin(); it != m_nodesIterator->second->edges->end(); it++)
		{
			//std::cout << "Writing Connection src::" << m_nodesIterator->first << " sink::" <<  it->first << std::endl;
			*r_addr_i = it->first; r_addr_i++; consumed += intsize;
			*r_addr_i = it->second->first->strength; r_addr_i++; consumed += intsize;
			r_addr_f = (float*) r_addr_i; 
			*r_addr_f = it->second->first->dot; r_addr_f++; consumed += floatsize;
			*r_addr_f = it->second->first->obb_overlap; r_addr_f++; consumed += floatsize;
			*r_addr_f = it->second->first->epsilon; r_addr_f++; consumed += floatsize;
			r_addr_i = (int*) r_addr_f;
			*r_addr_i = it->second->first->actions.size(); r_addr_i++; consumed += intsize;
			//std::cout << "ActionCount = " << it->second->first->actions.size() << std::endl;
			for (int i=0; i<it->second->first->actions.size(); i++)
			{
				*r_addr_i = it->second->first->actions[i]; r_addr_i++; consumed += intsize;
			}
		}	
		//std::cout << "Actual Bytes Written = " << consumed << " ... " << std::endl;
	}

	//Now write disjoint graphs initial states
	*r_addr_i = m_disjointStructureGraphs->Size(); r_addr_i++;
	for (int i=0; i<m_disjointStructureGraphs->Size(); i++)
	{
		*r_addr_i = m_disjointStructureGraphs->At(i)->id; r_addr_i++;
	}	

	//Now write disjoint graphs initial states
	*r_addr_i = m_mergedNodesMap->size(); r_addr_i++;
	for (std::map<int,int>::iterator it = m_mergedNodesMap->begin(); it != m_mergedNodesMap->end(); it++)
	{
		*r_addr_i = it->first; r_addr_i++;
		*r_addr_i = it->second; r_addr_i++;
	}

	*r_addr_i = m_mergedNodesSeeds->size(); r_addr_i++;
	for (std::map<int,std::vector<int>>::iterator it = m_mergedNodesSeeds->begin(); it != m_mergedNodesSeeds->end(); it++)
	{
		*r_addr_i = it->first; r_addr_i++;
		*r_addr_i = it->second.size(); r_addr_i++;
	}

	std::cout << "[StructureGraph        ] Writing Bin File " << pcsPath << ". WriteSize= [" << (Int64)r_addr_i - file_start << "]" << std::endl;		
}

bool StructureGraph::ReadFromBSTRFile(std::string _filepath)
{
	std::string pcsPath = _filepath+".bstr";		
	const char * filename = pcsPath.c_str();	
		
	boost::filesystem::path p (pcsPath, boost::filesystem::native);
	if ( !boost::filesystem::exists( p ) )
	{
		std::cerr << "Error : Couldn't open file '" << pcsPath << "'" << std::endl;
		return false;
	}
		
	//we assume all points/file fit in main memory
	boost::interprocess::file_mapping m_file;		
	boost::interprocess::mapped_region *m_region;
	int * r_addr_i;
	float * r_addr_f;	
	
	m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
	m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);					
	r_addr_f = (float*)m_region->get_address();
	r_addr_i = (int*)m_region->get_address();
	size_t r_size = m_region->get_size();					
	Int64 file_start = (Int64)r_addr_i;
	int numberOfNodes = (int)*r_addr_i; r_addr_i++;

	std::cout << "[StructureGraph        ] Reading Bin File " << pcsPath << ". FileSize= [" << r_size << "]" << std::endl;	
	
	std::map<int, std::vector<std::pair<int, EdgeStats *>>> nodeConnections;

	for (int i=0; i<numberOfNodes; i++) 
	{
		int id = (int)*r_addr_i; r_addr_i++;
		SegmentsGraph::SurfaceSegment * crtSurfaceSegment = m_segmentsGraph->m_surfaceSegments->at(id);
		CreateNewNode(crtSurfaceSegment);
		//std::cout << "ID - " << id << std::endl;
		Node * crtNode = m_nodes->at(id);
		crtNode->used = (int)*r_addr_i; r_addr_i++;
		//std::cout << "Used - " << crtNode->used << std::endl;
		crtNode->segment = m_segmentsGraph->m_surfaceSegments->at((int)*r_addr_i); r_addr_i++;
		//std::cout << "SegmentID - " << crtNode->segment->id << std::endl;
		crtNode->stats->point_count = (int)*r_addr_i; r_addr_i++;
		//std::cout << "PointCount - " << crtNode->stats->point_count << std::endl;
		crtNode->stats->shape = (shape)*r_addr_i; r_addr_i++;
		//std::cout << "Shape - " << crtNode->stats->shape << std::endl;
		crtNode->stats->type = (nodetype)*r_addr_i; r_addr_i++;
		//std::cout << "Type - " << crtNode->stats->type << std::endl;
				
		r_addr_f = (float*) r_addr_i;

		crtNode->stats->orientation.X = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->orientation.Y = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->orientation.Z = (float)*r_addr_f; r_addr_f++;

		crtNode->stats->bounding_min.X = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->bounding_min.Y = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->bounding_min.Z = (float)*r_addr_f; r_addr_f++;
				
		crtNode->stats->bounding_max.X = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->bounding_max.Y = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->bounding_max.Z = (float)*r_addr_f; r_addr_f++;

		crtNode->stats->mean_bounding_volume.X = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->mean_bounding_volume.Y = (float)*r_addr_f; r_addr_f++;
		crtNode->stats->mean_bounding_volume.Z = (float)*r_addr_f; r_addr_f++;		

		//std::cout << "Orientation:: " << crtNode->stats->orientation.ToString() << std::endl;
		//std::cout << "MeanBV:: " << crtNode->stats->mean_bounding_volume.ToString() << std::endl;
		
		r_addr_i = (int*) r_addr_f;
		int edgesCount = (int)*r_addr_i; r_addr_i++;

		//std::cout << "Node " << crtNode->id << " has " << edgesCount << " edges " << std::endl;

		for (int i=0; i<edgesCount; i++)
		{			
			EdgeStats * stats = new EdgeStats();
			int sinkNode = (int)*r_addr_i; r_addr_i++;
			//std::cout << "Reading Connection src::" << id << " sink::" << sinkNode << std::endl;
			stats->strength = (int)*r_addr_i; r_addr_i++;
			r_addr_f = (float*) r_addr_i;
			stats->dot = (float)*r_addr_f; r_addr_f++;
			stats->obb_overlap = (float)*r_addr_f; r_addr_f++;
			stats->epsilon = (float)*r_addr_f; r_addr_f++;
			r_addr_i = (int*) r_addr_f;
			int actionCount = (int)*r_addr_i; r_addr_i++;
			//std::cout << "ActionCount = " << actionCount << std::endl;
			for (int j=0; j<actionCount; j++)
			{				
				stats->actions.push_back((action)*r_addr_i);
				r_addr_i++;
			}
			//Temporarily store this node connection
			nodeConnections[id].push_back(std::pair<int, EdgeStats*>(sinkNode, stats));
		}		
	}

	//All nodes created - now update connectivity info
	for (std::map<int, std::vector<std::pair<int, EdgeStats *>>>::iterator nodeConnectionsIterator = nodeConnections.begin(); nodeConnectionsIterator != nodeConnections.end(); nodeConnectionsIterator++)
	{
		
		std::vector<std::pair<int, EdgeStats *>> conns = nodeConnectionsIterator->second;
		for (std::vector<std::pair<int, EdgeStats *>>::iterator conns_it = conns.begin(); conns_it != conns.end(); conns_it++)
		{

			(*(m_nodes->at(nodeConnectionsIterator->first)->edges))[conns_it->first] = new std::pair<EdgeStats*, Node*>(conns_it->second, m_nodes->at(conns_it->first));
		}
	}

	//Now read disjoint graphs initial states
	int numberOfDisjoint = (int)*r_addr_i; r_addr_i++;	
	for (int i=0; i<numberOfDisjoint; i++)
	{
		m_disjointStructureGraphs->PushBack(m_nodes->at((int)*r_addr_i)); r_addr_i++;		
	}	

		
	int mergedNodesMapSize = (int)*r_addr_i; r_addr_i++;	
	for (int i=0; i<mergedNodesMapSize; i++)
	{
		int s1 = (int)*r_addr_i; r_addr_i++;
		int s2 = (int)*r_addr_i; r_addr_i++;	
		(*m_mergedNodesMap)[s1] = s2;		
	}

	int merged_MapSize = (int)*r_addr_i; r_addr_i++;	
	for (int i=0; i<merged_MapSize; i++)
	{
		int s1 = (int)*r_addr_i; r_addr_i++;
		int s2 = (int)*r_addr_i; r_addr_i++;	//stores the number of nodes merged into this
		(*m_mergedNodesSeeds)[s1] = std::vector<int>();  //IMP: NEED TO FIX THIS.
	}	
	
	std::cout << "[StructureGraph        ] Reading Bin File " << pcsPath << ". ReadSize= [" << (Int64)r_addr_i - file_start << "]" << std::endl;
	return true;
	//std::cout << "[StructureGraph] :: Loaded Nodes = " << m_nodes->size() << std::endl;
	//std::cout << "[StructureGraph] :: Size Disjo = " << m_disjointStructureGraphs->Size() << std::endl;
	//std::cout << "[StructureGraph] :: Size MerMa = " << m_mergedNodesMap->size() << std::endl;
	//std::cout << "[StructureGraph] :: Size Merg_ = " << m_mergedNodes_->size() << std::endl;
}

void StructureGraph::Draw(std::string p_filepath, int p_parentSegmentId)
{		
	typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
	boost::char_separator<char> sep("/");
	tokenizer::iterator beg;
	
	std::string filename;
	tokenizer tok(p_filepath, sep);
	beg = tok.begin();
	//Discard all except for the last token
	while (beg != tok.end()) { filename = *beg; ++beg; }	

	std::string filepath = "Output/Graphs/"+boost::lexical_cast<std::string>(m_segmentsGraph->m_pcs->m_id)+"/"+boost::lexical_cast<std::string>(m_segmentsGraph->m_pcs->m_id)+".str";

	//First create a GraphViz file describing the graph
	std::cout << "[DrawSceneGraph] Drawing to " << filepath;
	std::ofstream graphfile;
	
	graphfile.open (filepath, std::ios::trunc);
	graphfile << "digraph G\n\t{\n";
	graphfile << "\t\tgraph [fontsize=8 fontname=\"Verdana\" compound=true];";
	graphfile << "\t\tnode [shape=oval fontsize=7 fontname=\"Verdana\"];\n";
	std::map<std::string, std::string> crtEdges;
	std::map<int, int>::iterator merged_nodes_iterator;	

	std::vector<StructureGraph::Node*> withInternalGraph;

	//First write the state names with some statistics
	for (std::map<int, StructureGraph::Node*>::iterator nods = m_nodes->begin(); nods!=m_nodes->end(); nods++)
	{		
		if (nods->second->segment->shape_primitive==SegmentsGraph::compound)
		{
			withInternalGraph.push_back(nods->second);
		}
		else
		{
			merged_nodes_iterator =  m_mergedNodesMap->find(nods->first);
			//Show only nodes which have not been merged.
			if (merged_nodes_iterator == m_mergedNodesMap->end())
			{
				if (nods->second->segment->shape_primitive==SegmentsGraph::plane)
					graphfile << "\t\t" << nods->first << "\t" << " [shape=\"parallelogram\" label=\"" << nods->first << "|" << nods->second->stats->point_count  << "\"]\n";
				else
					graphfile << "\t\t" << nods->first << "\t" << " [label=\"" << nods->first << "|" << nods->second->stats->point_count  << "\"]\n";
			}			
		}
	}

	graphfile << "\n\n";

	//output as separate subgraphs compound nodes, i.e. those having more than one node.
	StructureGraph * tmpStrGraph;
	StructureGraph::Node * tmpStrGraphNode;
	std::map<std::string, bool> tmpEdgeMap;
	for (int i = 0; i<withInternalGraph.size(); i++)
	{
		graphfile << "\tsubgraph " << withInternalGraph.at(i)->id << "\n\t{\n";
		graphfile << "\t\tlabel = " << withInternalGraph.at(i)->id << "\n";
		graphfile << "\t\tnode [style=filled]\n";
		tmpStrGraph = withInternalGraph.at(i)->segment->internalStrGraph;
		tmpEdgeMap.clear();
		for (int j = 0; j<tmpStrGraph->m_nodes->size(); j++)
		{			
			tmpStrGraphNode = tmpStrGraph->m_nodes->at(j);
			for (std::map<int, std::pair<StructureGraph::EdgeStats*, StructureGraph::Node*>*>::iterator toVisitIterator = tmpStrGraphNode->edges->begin(); 
				toVisitIterator != tmpStrGraphNode->edges->end(); toVisitIterator++)								
			{				
				std::string key;
				if (tmpStrGraphNode->id > toVisitIterator->second->second->id)
				{
					key = boost::lexical_cast<std::string>(tmpStrGraphNode->id);
					key.append(":");
					key.append(boost::lexical_cast<std::string>(toVisitIterator->second->second->id));				
				} 
				else
				{
					key = boost::lexical_cast<std::string>(toVisitIterator->second->second->id);
					key.append(":");
					key.append(boost::lexical_cast<std::string>(tmpStrGraphNode->id));					
				}
				if (tmpEdgeMap.find(key) != tmpEdgeMap.end()) break;
					
				tmpEdgeMap[key] = true;					
				graphfile << "\t\t" << withInternalGraph.at(i)->id << "." << tmpStrGraphNode->id << "->" << withInternalGraph.at(i)->id << "." << toVisitIterator->second->second->id << " [dir=none style=dotted]\n";
			}
		}
		graphfile << "\t}\n";
	}
	
	StructureGraph::Node * crtNode;
	std::string k1;
	std::string k2;
	std::string n1;
	std::string n2;

	for (int i=0; i<m_disjointStructureGraphs->Size(); i++)
	{
		//graphfile << "\tsubgraph cluster" << i << "\n\t{\n";
		Illumina::Core::List<int> * nodesList = BreadthFirstTraversal(m_disjointStructureGraphs->At(i));
		for (int j=0; j<nodesList->Size(); j++)
		{
			if (m_nodes->find(nodesList->At(j)) != m_nodes->end())
				crtNode = m_nodes->at(nodesList->At(j));
			else break;
			for (std::map<int, std::pair<StructureGraph::EdgeStats*, StructureGraph::Node*>*>::iterator toVisitIterator = crtNode->edges->begin(); toVisitIterator != crtNode->edges->end(); toVisitIterator++)		
			{
				k1.clear();
				k2.clear();
				n1.clear();
				n2.clear();

				if (crtNode->id > toVisitIterator->first)
				{
					n1 = boost::lexical_cast<std::string>(crtNode->id);
					if (crtNode->segment->internalStrGraph!=NULL) n1.append(".0");
					n2 = boost::lexical_cast<std::string>(toVisitIterator->first);
					if (toVisitIterator->second->second->segment->internalStrGraph!=NULL) n2.append(".0");
					k1 = n1.append("->"); k1.append(n2);
				}
				else
				{
					n1 = boost::lexical_cast<std::string>(crtNode->id);
					if (crtNode->segment->internalStrGraph!=NULL) n1.append(".0");
					n2 = boost::lexical_cast<std::string>(toVisitIterator->first);
					if (toVisitIterator->second->second->segment->internalStrGraph!=NULL) n2.append(".0");
					k1 = n2.append("->"); k1.append(n1);
				}			

				if (crtNode->segment->internalStrGraph!=NULL && toVisitIterator->second->second->segment->internalStrGraph!=NULL)
				{
					k2.append(" [dir=none lhead="); k2.append(boost::lexical_cast<std::string>(crtNode->id)); 
					k2.append(", ltail="); k2.append(boost::lexical_cast<std::string>(toVisitIterator->first));
				}

				if (crtNode->segment->internalStrGraph!=NULL && toVisitIterator->second->second->segment->internalStrGraph==NULL)
				{
					k2.append(" [dir=none lhead="); k2.append(boost::lexical_cast<std::string>(crtNode->id));
				}

				if (toVisitIterator->second->second->segment->internalStrGraph!=NULL && crtNode->segment->internalStrGraph==NULL)
				{
					k2.append(" [dir=none ltail="); k2.append(boost::lexical_cast<std::string>(toVisitIterator->first));
				}

				if (toVisitIterator->second->second->segment->internalStrGraph==NULL && crtNode->segment->internalStrGraph==NULL)
				{					
					k2.append(" [dir=none");					
				}								

				float dot = toVisitIterator->second->first->dot;
				dot = Maths::Abs(dot);
				//std::cout << crtNode->id << " <DOT-" << dot << "-DOT> " << toVisitIterator->second->second->id << std::endl;
				if (dot>0.96f && dot<1.04f)
				{
					k2.append(" label="); k2.append(boost::lexical_cast<std::string>(dot)); k2.append("-"); k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->strength));k2.append(" penwidth=4 color=\"black\"]");
				}
				else 
				{
					if (dot>-0.04f && dot<0.04f)
					{
						k2.append(" label="); k2.append(boost::lexical_cast<std::string>(dot)); k2.append("-"); k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->strength));k2.append(" penwidth=3 color=\"blue\"]");
					} 
					else
					{
						k2.append(" label="); k2.append(boost::lexical_cast<std::string>(dot)); k2.append("-"); k2.append(boost::lexical_cast<std::string>(toVisitIterator->second->first->strength));k2.append(" penwidth=2 color=\"orange\"]");
					}
				}
				crtEdges[k1] = k2;						
			}
		}

		for (std::map<std::string, std::string>::iterator niter = crtEdges.begin(); niter != crtEdges.end(); niter++)
		{
			graphfile << "\t\t" << niter->first << " " << niter->second << "\n";
		}

		crtEdges.clear();  //clear the map for the next disjoint subgraph
		//graphfile << "\t}\n";	
	}
		
	graphfile << "}";
	graphfile.close();
	std::cout << " .. [done]" << std::endl;	
}
